#include"Motor.h"
#define PERIOD MAIN_Fosc/1000
void Motor_GPIO()
{
	GPIO_InitTypeDef	GPIO_InitStructure;		//结构定义
	GPIO_InitStructure.Pin  = GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;		//指定要初始化的IO,
	GPIO_InitStructure.Mode = GPIO_PullUp;	//指定IO的输入或输出方式,GPIO_PullUp,GPIO_HighZ,GPIO_OUT_OD,GPIO_OUT_PP
	GPIO_Inilize(GPIO_P1, &GPIO_InitStructure);//初始化

    GPIO_InitStructure.Pin  = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3;		//指定要初始化的IO,
	GPIO_InitStructure.Mode = GPIO_PullUp;	//指定IO的输入或输出方式,GPIO_PullUp,GPIO_HighZ,GPIO_OUT_OD,GPIO_OUT_PP
	GPIO_Inilize(GPIO_P2, &GPIO_InitStructure);//初始化
}

u8 speed2duty(int speed)
{
    return speed/2+50;
}
void Motor_PWM_Config(MotorSpeed ms)
{
    u8 LQ_Duty =speed2duty(ms.LQ_Speed);
    u8 LH_Duty =speed2duty(ms.LH_Speed);
    u8 RQ_Duty =speed2duty(ms.RQ_Speed);
    u8 RH_Duty =speed2duty(ms.RH_Speed);
    PWMx_InitDefine init;
	init.PWM_Period=PERIOD -1;		//周期时间,   0~65535
	init.PWM_DeadTime=100;	//死区发生器设置, 0~255
	init.PWM_CEN_Enable=ENABLE;		//使能计数器, ENABLE,DISABLE
	init.PWM_MainOutEnable=ENABLE;//主输出使能,  ENABLE,DISABLE
	//左前轮
    init.PWM_Mode=CCMRn_PWM_MODE1;
    init.PWM_EnoSelect=ms.LQ_Speed ? ENO3P|ENO3N : 0;
    init.PWM_Duty=PERIOD*LQ_Duty;
    PWM_Configuration(PWM3,&init);
    //左后轮
    init.PWM_Mode=CCMRn_PWM_MODE1;
    init.PWM_EnoSelect=ms.LH_Speed ? ENO1P|ENO1N : 0;
    init.PWM_Duty=PERIOD*LH_Duty;
    PWM_Configuration(PWM1,&init);
    //右前轮
    init.PWM_Mode=CCMRn_PWM_MODE2;
    init.PWM_EnoSelect=ms.RQ_Speed ? ENO4P|ENO4N : 0;
    init.PWM_Duty=PERIOD*RQ_Duty;
    PWM_Configuration(PWM4,&init);
    //右后轮
    init.PWM_Mode=CCMRn_PWM_MODE2;
    init.PWM_EnoSelect=ms.RH_Speed ? ENO2P|ENO2N : 0;
    init.PWM_Duty=PERIOD*RH_Duty;
    PWM_Configuration(PWM2,&init);
    PWM_Configuration(PWMA,&init);

    PWM1_SW(PWM1_SW_P20_P21);
    PWM2_SW(PWM2_SW_P22_P23);
    PWM3_SW(PWM3_SW_P14_P15);
    PWM4_SW(PWM4_SW_P16_P17);
}

void Motor_Init()
{
    Motor_GPIO();
    EAXSFR();
}

void Motor_Forward(int8 speed,MotorMode mode)
{
    MotorSpeed ms;

    if(mode==MODE_LEFT)
    {//左前
        ms.LQ_Speed=0;
        ms.LH_Speed=speed;
        ms.RQ_Speed=speed;
        ms.RH_Speed=0;
    }
    else if(mode==MODE_MID)
    {//正前
        ms.LQ_Speed=speed;
        ms.LH_Speed=speed;
        ms.RQ_Speed=speed;
        ms.RH_Speed=speed;
    }
    else if(mode==MODE_RIGHT)
    {
        ms.LQ_Speed=speed;
        ms.LH_Speed=0;
        ms.RQ_Speed=0;
        ms.RH_Speed=speed;
    }
    Motor_PWM_Config(ms);
}//0.5+0.05*v;

void Motor_Backward(int8 speed,MotorMode mode)
{
    MotorSpeed ms;

    if(mode==MODE_LEFT)
    {
        ms.LQ_Speed=-speed;
        ms.LH_Speed=0;
        ms.RQ_Speed=0;
        ms.RH_Speed=-speed;
    }
    else if(mode==MODE_MID)
    {
        ms.LQ_Speed=-speed;
        ms.LH_Speed=-speed;
        ms.RQ_Speed=-speed;
        ms.RH_Speed=-speed;
    }
    else if(mode==MODE_RIGHT)
    {
        ms.LQ_Speed=0;
        ms.LH_Speed=-speed;
        ms.RQ_Speed=-speed;
        ms.RH_Speed=0;
    }
    
    Motor_PWM_Config(ms);
}

void Motor_Left(u8 speed)
{
    MotorSpeed ms;
    ms.LQ_Speed=0;
    ms.LH_Speed=0;
    ms.RQ_Speed=speed;
    ms.RH_Speed=speed;
}

void Motor_Right(u8 speed)
{
    MotorSpeed ms;
    ms.LQ_Speed=speed;
    ms.LH_Speed=speed;
    ms.RQ_Speed=0;
    ms.RH_Speed=0;
}

void Motor_Translate(int8 speed,MotorMode mode)
{
    MotorSpeed ms;
    if(mode==MODE_LEFT)//左平移
    {
        ms.LQ_Speed=-speed;
        ms.LH_Speed=speed;
        ms.RQ_Speed=speed;
        ms.RH_Speed=-speed;
    }
    else if(mode==MODE_RIGHT)//右平移
    {
        ms.LQ_Speed=speed;
        ms.LH_Speed=-speed;
        ms.RQ_Speed=-speed;
        ms.RH_Speed=speed;
    }

    Motor_PWM_Config(ms);
}

void Motor_Around(int8 speed,MotorMode mode)
{
    MotorSpeed ms;
    if(mode==MODE_LEFT)//左旋转
    {
        ms.LQ_Speed=speed;
        ms.LH_Speed=speed;
        ms.RQ_Speed=-speed;
        ms.RH_Speed=-speed;
    }
    else if(mode==MODE_RIGHT)//右旋转
    {
        ms.LQ_Speed=-speed;
        ms.LH_Speed=-speed;
        ms.RQ_Speed=speed;
        ms.RH_Speed=speed;
    }
}

int limit_speed(int speed)
{//摇杆操控坐标如果x与y算术值超出速度范围时将其调回边界
    if(speed>=100)return 100;
    else if(speed <-100)return -100;

    else return speed;
}

void Motor_Move(int x,int y)
{
    MotorSpeed ms;
    ms.LQ_Speed=limit_speed(y+x) *0.3;//0.3是为了限制速度
    ms.LH_Speed=limit_speed(y-x) *0.3;
    ms.RQ_Speed=limit_speed(y-x) *0.3;
    ms.RH_Speed=limit_speed(y+x) *0.3;

    Motor_PWM_Config(ms);
}

void Motor_Stop()
{
    MotorSpeed ms;
    ms.LQ_Speed=0;
    ms.LH_Speed=0;
    ms.RQ_Speed=0;
    ms.RH_Speed=0;

    Motor_PWM_Config(ms);
}